/*
    Copyright (c) 2009 Pavol Malosek a.k.a. malo

    This file is part of ComComLib.

    ComComLib is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 3 of the License, or
    (at your option) any later version.

    ComComLib is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <cmath>
#include <cstdio>
#include <assert.h>
#include <cstring>

#include <lcc/dlmath.hpp>

#define LCC_MATH_DEBUG
#define LCC_MATH_DEBUG_LEVEL 2

// level 1 = key behaviour
// level 2 = processing flow
// level 4 = infos

#ifndef LCC_MATH_DEBUG
#   define lcc_log(n, ...)  while (0)
#else
#   define lcc_log(n, ...)    do { if ((n) <= LCC_MATH_DEBUG_LEVEL) \
                                { printf (__VA_ARGS__);}} while (0)
#endif


unsigned int lcc::act_bits (uint8_t byte_)
{
    unsigned int act_bits = 0;

    for (unsigned int i = 0; i < 8; i++) {
        if (byte_ & 0x1)
            act_bits++;

        byte_ = byte_ >> 1;
    }
    
    lcc_log (4, "byte 0x%xh has %i one-s\n", byte_, act_bits);

    return act_bits;
}

float lcc::dot_float_to_float (uint8_t exp_, uint8_t b0_, uint8_t b1_, 
    uint8_t b2_)
{
    
    lcc_log (4, " exp 0x%x, B0 0x%x, B1 0x%x, B2 0x%x, %s(%i)\n",
        exp_, b0_, b1_, b2_, __FILE__, __LINE__);

    //  Make sure that float has 4B.
    assert (sizeof (float) == 4);

    //  Store sign.
    bool sign = b0_ & 0x80;
    
    //  Reset MSB from b0    
    b0_ &= 0x7f;

    float pc_float = 0;

    //  Probably not fitting with logger doc.
    if (exp_ != 0 || b2_ != 0 || b1_ != 0 || b0_ != 0) {
        uint32_t buff =  b2_ | b1_ << 8 | b0_ << 16 | (exp_ - 0x1) << 23 | sign << 31;
        memcpy (&pc_float, &buff, sizeof (float));
    }

    lcc_log (4, "float format %.13f, %s(%i)\n", pc_float, __FILE__, __LINE__);

    return pc_float;
}

uint8_t lcc::bcd_to_uint8 (uint8_t bcd_value_)
{
    uint8_t lo_4 = bcd_value_ & 0x0f;
    uint8_t hi_4 = (bcd_value_ & 0xf0) >> 4;
    return lo_4 + 10 * hi_4;;
}

float lcc::calc_temp_Pt_1000 (lcc::calc_constants_t *constants_, 
    unsigned int adc_value_)
{

    float R_rtd = adc_value_ * constants_->k_n_arg + constants_->q_n_arg;
    float R = R_rtd * constants_->k_n_usr + constants_->q_n_usr; 
 
    float p = R / 1000;
    float q = 3383.805230592;

    lcc_log (4, "R %.5f, p %.5f, q %.5f\n", R, p, q);

    float T = 0;

    if (p < 1) {

        float part_a = -241.96560251;
        float part_b = 2.2163854354e2 * p; 
        float part_c = 2.8548406119e1 * pow (p, 2);
        float part_d = 9.960255151 * pow (p, 3);
        float part_e = 1.7389080065 * pow (p, 4);
        
        lcc_log (4, "part_a %.5f, part_b %.5f, part_c %.5f, part_d %.5f,"\
            "part_e %.5f\n", part_a, part_b, part_c, part_d, part_e);

        T = part_a + part_b + part_c + part_d + part_e;

    } else {
        float pt = (q * q - (p - 1) / 5.775e-7);

        lcc_log (4, "pt %.5f\n", pt);        

        T = q - sqrt (pt);
    }
    
    lcc_log (4, "T %.1f, %s(%i)\n", T, __FILE__, __LINE__);

    return T;
}

float lcc::calc_hum (lcc::calc_constants_t *constants_, float T_,
    unsigned int adc_value_, bool valid_T_)
{

    lcc_log (4, "k_arg %.5f, q_arg, %.5f, %s(%i)\n", constants_->k_n_arg, 
            constants_->q_n_arg, __FILE__, __LINE__);
    lcc_log (4, "T %.1f, ADC value %i, %s(%i)\n", T_, adc_value_, 
        __FILE__, __LINE__);

    float H_mer = 1.00 / (float)adc_value_ * constants_->k_n_arg + constants_->q_n_arg;

    lcc_log (4, "H_mer %.5f\n", H_mer);

    float H_lin = 0;

    //  Linearisation.
    if (H_mer < 10) {
        H_lin = H_mer * 0.715 + 2.81;
    } else if (H_mer >= 10 && H_mer < 34) {
        H_lin = H_mer * 0.9482758 + 0.57;
    } else {
        H_lin = H_mer * 1.029411 - 2.2;
    }

    lcc_log (4, "H_lin %.5f\n", H_lin);

    float c = 0;
    float d = 0;
    
    if (!valid_T_)
        goto out;

    //  Temperature compensation.
    if (T_ <= -10) {

        if (H_lin < 30) {
            c = 0.0049 * T_ - 0.029;
            d = -0.0435 * T_ - 1.57;
        } else {
            c = 0.000996 * T_ - 0.0256;
            d = 0.0789 * T_ - 1.66;
        }

    } else if ( T_ > -10 && T_ <= 30) {
 
            c = 0.000996 * T_ - 0.0256;
            d = 0.0789 * T_ - 1.66; 
 
    } else if (T_ > 30 && T_ <= 80) {
        
        if (H_lin < 30) {
            c = 0.00067 * T_ - 0.01;
            d = 0.1055 * T_ - 2.61;
        } else {
            c = 0.000264 * T_ - 0.00358;
            d = 0.1143 * T_ - 2.72;
        }

    } else {

        if (H_lin < 0) {
            c = -0.0035 * T_ - 0.557;
            d = 0.083 * T_ - 0.042;
        } else {
            c = 0.019;
            d = 0.0857 * T_ - 0.4;
        }

    }
   
out: 
    lcc_log (4, "c %.5f, d %.5f\n", c, d);

    float H = H_lin + H_lin * c + d;
    
    lcc_log (4, "H %.1f, %s(%i)\n", H, __FILE__, __LINE__);

    return H;
}

void lcc::adc_to_real_values (device_info_t *device_info_, 
    std::list<record_t> &records_)
{

    std::list<record_t>::iterator it = records_.begin ();

    while (it != records_.end ()) {
        //  We have to know if to use T compensation for humidity.
        bool have_valid_temp = false;
        float T = 0;
        float H = 0;
        
        for (unsigned int i = 0; i < max_channels; i++) {

            switch (it->values [i].input_type) {
                case in_temp_pt:
                    //  Calculate temerature for pt 1000 sensor.
                    T = calc_temp_Pt_1000 (&(device_info_->calc_constants [i]),
                        it->values [i].adc_value);
                    
                    it->values [i].value = T;

                    have_valid_temp = true;

                    break;
                case in_hum:
                    //  Calculate humidity.
                    H = calc_hum (&(device_info_->calc_constants [i]), T, 
                        it->values [i].adc_value, have_valid_temp);
                         
                    it->values [i].value = H;

                    break;
                case in_off:
                    break;
                default:
                    assert (false);
            } 
        }

        //  Next record.
        it++;
    }
}

